/*
 * CameraGUI.cpp
 *
 *  Created on: Mar 23, 2013
 *      Author: ohnozzy
 */
#include <GL/glut.h>
#include "flyingcamera/base/CameraSpace.h"
#include "flyingcamera/conversion/Conversion.h"
#include "GUI.h"
#include "moving/MCameraGUI.h"
#include <iostream>

flyingcamera::gui::CameraGUI* cgui;
bool loop_enable=false;
unsigned location=0;
void flyingcamera::gui::DisableLoop(){
	loop_enable=false;
}
void flyingcamera::gui::setCameraGUI(flyingcamera::gui::CameraGUI* g){
	cgui=g;
}
void flyingcamera::gui::show(int* argc, char** argv){

	glutInit(argc, argv);
	glutInitDisplayMode (GLUT_RGB| GLUT_DEPTH);
	glutInitWindowSize (500, 500);
	glutInitWindowPosition (100, 100);
	glutCreateWindow (argv[0]);
	glClearColor (0.0, 0.0, 0.0, 0.0);
		//glEnable(GL_CULL_FACE);
    glEnable(GL_DEPTH_TEST);
	glDepthMask(GL_TRUE);
	glutDisplayFunc(display);
	glutReshapeFunc(reshape);
	glutTimerFunc (1, timer_function,0);
	glDepthFunc(GL_LEQUAL);
	glutMainLoop();
}
void flyingcamera::gui::reshape (int w, int h)
{
   glViewport (0, 0, (GLsizei) w, (GLsizei) h);
   glMatrixMode (GL_PROJECTION);
   glLoadIdentity ();
   //glFrustum (-1.0, 1.0, -1.0, 1.0, cgui->distance/2, cgui->distance*4);
   gluPerspective(39.6, ((double)w)/((double)h),cgui->distance/10, cgui->distance*2);
   glMatrixMode (GL_MODELVIEW);
}
void flyingcamera::gui::timer_function( int value)
{
	if(value>0){
	   location++;
	   if(location>=cgui->getStateCount()){
		   if(loop_enable)
		      location=0;
		   else{
			   location--;
			   return;
		   }
	   }
 }

 double delay=cgui->getStateDelay(location);

 glutPostRedisplay();

 glutTimerFunc( ceil(10000*delay),timer_function , 1);
}
void flyingcamera::gui::display(){
	const ompl::base::State* state=cgui->getState(location);
	const base::CameraSpace::StateType* cstate=state->as<base::CameraSpace::StateType>();
	double x,y,yaw;
	cgui->path_converter->getLocation(cstate->getT(),x,y,yaw);
	//std::cout<<"robot ("<<cstate->getT()<<"): "<<x<<","<<y<<std::endl;
	fcl::Quaternion3f q;
    q.getX()=cstate->rotation().x;
    q.getY()=cstate->rotation().y;
    q.getZ()=cstate->rotation().z;
    q.getW()=cstate->rotation().w;
    fcl::Transform3f tf(q);
    flyingcamera::conversion::moveUpandTranslate(tf, cgui->distance, x, y);
    glClear (GL_COLOR_BUFFER_BIT| GL_DEPTH_BUFFER_BIT);
    glLoadIdentity ();
	setCamera(tf, x, y);
    //gluLookAt (x, y-1, 0, x, y, 0.0, 0, 0, 1);
	displayRobot(x, y);
	displayEnvironment();
	if(cgui->hasMovingObstacle())
		displayMObstacle(cstate->getT());
    glFlush();

}
void flyingcamera::gui::displayRobot(double x, double y){

	glPushMatrix();
	glColor3f (0.0, 0.0, 1.0);
	glTranslatef(x, y, 0.05);
	glutSolidCube (0.1);
    glPopMatrix();
}
void flyingcamera::gui::setCamera(const fcl::Transform3f& tf, double x, double y){
	const fcl::Matrix3f& m=tf.getRotation();
	const fcl::Vec3f& tran=tf.getTranslation();
	double half_length=cgui->distance/2;
	double cx=half_length*m(0,2)+tran[0];
	double cy=half_length*m(1,2)+tran[1];
	double cz=half_length*m(2,2)+tran[2];
	double dx=0;
	double dy=0;
	double dz=1;
	if(cz>0.99*cgui->distance){
			dy=1;
			dz=0;
		}
	gluLookAt (cx, cy, cz, x, y, 0.0, dx, dy, dz);

}
void flyingcamera::gui::displayObject(const fcl::Box& o, const fcl::Transform3f& tf, const fcl::Vec3f& color){
	            const fcl::Vec3f& translate=tf.getTranslation();
				fcl::Vec3f axis;
				fcl::FCL_REAL angle;
				tf.getQuatRotation().toAxisAngle(axis, angle);
				glPushMatrix();
			    glColor3f (color[0], color[1], color[2]);
				glTranslatef(translate[0], translate[1], translate[2]);
				glRotatef((double)angle*180/3.14, axis[0], axis[1], axis[2]);
	            glScalef(o.side[0],o.side[1],o.side[2]);
				glutSolidCube (1);
			    glPopMatrix();
}
void flyingcamera::gui::displayEnvironment(){
	for (unsigned i=0; i<cgui->getObstacleCount(); i++){
		    fcl::Transform3f tf;
		    fcl::Box b;
		    fcl::Vec3f c;
		    cgui->getObstacle(i, b, tf, c);
		    displayObject(b, tf, c);
			/*const fcl::Vec3f& translate=tf.getTranslation();
			fcl::Vec3f axis;
			fcl::FCL_REAL angle;
			tf.getQuatRotation().toAxisAngle(axis, angle);
			glPushMatrix();
		    glColor3f (c[0], c[1], c[2]);
			glTranslatef(translate[0], translate[1], translate[2]);
			glRotatef((double)angle*180/3.14, axis[0], axis[1], axis[2]);
            glScalef(b.side[0],b.side[1],b.side[2]);
			glutSolidCube (1);
		    glPopMatrix();*/
	}
}

void flyingcamera::gui::displayMObstacle(double t){
	flyingcamera::gui::moving::MCameraGUI* mgui=static_cast<flyingcamera::gui::moving::MCameraGUI*>(cgui);
	for (unsigned i=0; i<mgui->getMObstacleCount(); i++){
			    fcl::Transform3f tf;
			    fcl::Box b;
			    fcl::Vec3f c;
			    mgui->getMObstacle(i, t, b, tf, c);
			    const fcl::Vec3f& translate=tf.getTranslation();
			    //std::cout<<"obstacle: "<<translate[0]<<", "<<translate[1]<<", "<<translate[2]<<std::endl;
			    displayObject(b, tf, c);
	}
}

